// NOTE OF USAGE: First movement must be pitch up/down to avoid singularity. 
// v1.0 27/06/13

#include "opencv2/objdetect/objdetect.hpp"
//#include "opencv2/features2d/features2d.hpp"
#include "opencv2/nonfree/features2d.hpp"     // SURF & SIFT headers have been moved here!!!!
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc/imgproc_c.h"

#include <iostream>
#include <vector>
#include <stdio.h>
#include <time.h>

#include "global.h"

#include "MatWork.hpp"
#include "DENSOrobot.hpp"


using namespace std;
using namespace cv;


//DENSOrobot DSrb = DENSOrobot(1);   // 1 to run real robot

//struct DSpose 
//{
//	double x, y, z, rol, pit, yaw;		
//};
//
//typedef DSpose CPose;


int moveDENSO(DENSOrobot& DSrb, const CPose myCPose)
{

	Mat tmp;
	Mat BestTraj;

	tmp = Mat::zeros(1,6,CV_64F);

	tmp.at<double>(0) = myCPose.x;
	tmp.at<double>(1) = myCPose.y;
	tmp.at<double>(2) = myCPose.z;
	tmp.at<double>(3) = myCPose.rol;
	tmp.at<double>(4) = myCPose.pit;
	tmp.at<double>(5) = myCPose.yaw;

	int ret = DSrb.TrajPlanner(DSrb.JPose, tmp, BestTraj);  

	if (ret)
	{
		//MATmtxdisp(traj/CV_PI*180);
		//DstPose.copyTo(OrgPoseJnt);

		//getchar();
	}
	else 
	{
		printf("there is no good solution!!\n");
		//getchar();
		//return 1;	
	}

	return ret;
}

int CPoseOut(const CPose myCPose)
{
	cout << "current pose is:" << endl;
	cout << "[";
	cout << myCPose.x << ", "; 
	cout << myCPose.y << ", "; 
	cout << myCPose.z << ", "; 
	cout << myCPose.rol << ", "; 
	cout << myCPose.pit << ", "; 
	cout << myCPose.yaw << "]"; 
	cout << endl;

	return 1;
}

int main(int argc, char** argv)
{
	
	// #### DENSO control vars
	DENSOrobot DSrb = DENSOrobot(1);   // 1 to run real robot

	// Initial Joint Pose - always start from this pose
	Mat OrgPose = Mat::zeros(1, 6, CV_64F);
	//Mat OrgPose = Mat::zeros(1, 6, CV_64F, 0);   // WRONG SYNTAX!!!! init Mat with Mat::zeros
	Mat DstPose = Mat::zeros(1, 6, CV_64F);   // used later
	Mat OrgPoseJnt, traj;

	double OrgPose_ar1[6] = {0, -CV_PI/6, 2*CV_PI/3, 0, 0, 0};
	OrgPose = Mat(1, 6, CV_64F, OrgPose_ar1);	
	OrgPose.copyTo(OrgPoseJnt);

	// update pose to DENSO robot
	DSrb.updateJPose(OrgPose);

	double rol=0, pit=0, yaw=0;   // fix roll-pitch-yaw
	Mat BestTraj;
	// ################################

	// ****** initial SETUP 
	Mat tmp = Mat::zeros(1,6,CV_64F);
	// ****** START Move to initial Pose
	// 0, -CV_PI/6, 2*CV_PI/3, 0, 0, 0
	tmp.at<double>(0) = 0;
	tmp.at<double>(1) = -CV_PI/6.0;
	tmp.at<double>(2) = 2.0*CV_PI/3.0;
	tmp.at<double>(3) = 0;
	tmp.at<double>(4) = 0;
	tmp.at<double>(5) = 0;    // tmp here is 6x1 mat

	int ret = DSrb.JTrajPlanner(tmp);  // TrajPlanner works with row vector

	if (ret)
	{
		//MATmtxdisp(traj/CV_PI*180);
		//DstPose.copyTo(OrgPoseJnt);

		//getchar();
	}
	else 
	{
		printf("there is no good solution!!\n");
		//getchar();
		//return 1;
	}

	//getchar();
	// ****** END Move to initial Pose
	
	// #### DENSO keyboard control vars
	namedWindow("DENSO keyboard");
	//Mat tmp;
	char c;
	double trans_step = 10; // mm
	double rot_step = 5.0/180.0*CV_PI;
	CPose myCPose;
	//DSrb.getCPose(myCPose);  // singular here!!
	myCPose.x = 175;
	myCPose.y = 0;
	myCPose.z = 256.865;
	// rol pit yaw - offsetted
	myCPose.rol = 0; //CV_PI;
	myCPose.pit = 0; //-CV_PI/2.0;
	myCPose.yaw = 0;

	cout << myCPose.x << endl;
	cout << myCPose.y << endl;
	cout << myCPose.z << endl;
	cout << myCPose.rol << endl;
	cout << myCPose.pit << endl;
	cout << myCPose.yaw << endl;
	//getchar();
	int init_flg = 1;   // flag of initial step to avoid singularity. First movement should be pitch up/down
	// #### DENSO keyboard control vars

	while (1)
	{
		c = waitKey(1);

		// ****** START Move to initial Pose
		switch (c)
		{
			// CONTROL TRANSLATION
			case 'a': 
				// get current CPose
				if (!init_flg)
					DSrb.getCPose(myCPose);
				else
					init_flg = 0;

				// add according increment
				myCPose.y = myCPose.y + trans_step; 
				// move robot to next CPose
				moveDENSO(DSrb, myCPose);
				CPoseOut(myCPose);
				break;

			case 'd':
				// get current CPose
				if (!init_flg)
					DSrb.getCPose(myCPose);
				else
					init_flg = 0;

				// add according increment
				myCPose.y = myCPose.y - trans_step; 
				// move robot to next CPose
				moveDENSO(DSrb, myCPose);
				CPoseOut(myCPose);
				break;

			case 'w':
				// get current CPose
				if (!init_flg)
					DSrb.getCPose(myCPose);
				else
					init_flg = 0;

				// add according increment
				myCPose.x = myCPose.x + trans_step; 
				// move robot to next CPose
				moveDENSO(DSrb, myCPose);
				CPoseOut(myCPose);
				break;
			
			case 'x':
				// get current CPose
				if (!init_flg)
					DSrb.getCPose(myCPose);
				else
					init_flg = 0;

				// add according increment
				myCPose.x = myCPose.x - trans_step; 
				// move robot to next CPose
				moveDENSO(DSrb, myCPose);
				CPoseOut(myCPose);
				break;
				
			case 'q':
				// get current CPose
				if (!init_flg)
					DSrb.getCPose(myCPose);
				else
					init_flg = 0;

				// add according increment
				myCPose.z = myCPose.z - trans_step; 
				// move robot to next CPose
				moveDENSO(DSrb, myCPose);
				CPoseOut(myCPose);
				break;

			case 'e':
				// get current CPose
				if (!init_flg)
					DSrb.getCPose(myCPose);
				else
					init_flg = 0;

				// add according increment
				myCPose.z = myCPose.z + trans_step; 
				// move robot to next CPose
				moveDENSO(DSrb, myCPose);
				CPoseOut(myCPose);
				break;

				// CONTROL ROTATION
			case '4':
				// get current CPose
				if (!init_flg)
					DSrb.getCPose(myCPose);
				else
					init_flg = 0;

				// add according increment
				myCPose.yaw = myCPose.yaw - rot_step; 
				// move robot to next CPose
				moveDENSO(DSrb, myCPose);
				CPoseOut(myCPose);
				break;
				
			case '6':
				// get current CPose
				if (!init_flg)
					DSrb.getCPose(myCPose);
				else
					init_flg = 0;

				// add according increment
				myCPose.yaw = myCPose.yaw + rot_step; 
				// move robot to next CPose
				moveDENSO(DSrb, myCPose);
				CPoseOut(myCPose);
				break;

			case '8':
				// get current CPose
				if (!init_flg)
					DSrb.getCPose(myCPose);
				else
					init_flg = 0;

				// add according increment
				myCPose.pit = myCPose.pit + rot_step; 
				// move robot to next CPose
				moveDENSO(DSrb, myCPose);
				CPoseOut(myCPose);
				break;

			case '2':
				// get current CPose
				if (!init_flg)
					DSrb.getCPose(myCPose);
				else
					init_flg = 0;

				// add according increment
				myCPose.pit = myCPose.pit - rot_step; 
				// move robot to next CPose
				moveDENSO(DSrb, myCPose);
				CPoseOut(myCPose);
				break;

			case '7':
				// get current CPose
				if (!init_flg)
					DSrb.getCPose(myCPose);
				else
					init_flg = 0;

				// add according increment
				myCPose.rol = myCPose.rol - rot_step; 
				// move robot to next CPose
				moveDENSO(DSrb, myCPose);
				CPoseOut(myCPose);
				break;

			case '9':
				// get current CPose
				if (!init_flg)
					DSrb.getCPose(myCPose);
				else
					init_flg = 0;

				// add according increment
				myCPose.rol = myCPose.rol + rot_step; 
				// move robot to next CPose
				moveDENSO(DSrb, myCPose);
				CPoseOut(myCPose);
				break;
			
			default: break;
				//cout << "Wrong Key!!" << endl;
		}
	
		if (c==27) break;
		else	
		{
		
		} 
	}

	return 0;
}